package Behaviors.Schemas.Perception;
import Robot.*;
import Robot.Sensors.Sensor;
import javaclient3.*;
import javaclient3.structures.*;
import Utils.*;


//TODO: Corregir que este ubicando los obstaculos con referencia global. Dejarlo como referencia
//local
/**
 * Class implementing a Perception Schema (PS) for dealing with sonar readings.
 *
 * This class converts sonar readings into vector readings, for easy use with Motor Schemas (MS).
 *
 * @author Alejandro Pustowka
 */
public class NxPSDetectObstaclesSonarNew extends NxPSDetectObstacles {

    /**
     * The SonarInterface from Javaclient
     */
    SonarInterface sonar;


    Position2DInterface pos;

    /**
     * Defines wether the sonar should behave as an infrarred sensor
     * that means, no distance measuring, only detection of obstacles.
     */
    boolean irMode = false;


    /**
     * Creates a Perception Schema based on Sonar readings.
     *
     * The output of this schema is an Array of Vector2d, each referenced to
     * the robots origin. That means, this schema is heavily based on localization
     * information. It has to be used with a Position2DInterface. In coming versions,
     * this could be fixed.
     * @param name Name of this Schema.
     * @param pos The Position2DInterface of Jaclient, from where the localization will be known.
     * @param sonar The SonarInterface of Javaclient, from where the readings come.
     * @param irMode Sets the behavior of the Sonar readings (to be treated as an IR sensor).
     */
    public NxPSDetectObstaclesSonarNew(String name,Position2DInterface pos, PlayerDevice sonar1,boolean irMode) {

        super(name);

        //Assign Player Interfaces
        this.sonar = (SonarInterface)sonar1;
        this.pos = pos;

        //set sonar behavior
        this.irMode = irMode;

        //query poses of the sonars
        sonar.queryGeometry();

        //wait until geometry is ready
        while(!sonar.isGeomReady());

        //get number of sonars, equ. to number of readings of this sensor
        numReadings = sonar.getGeom().getPoses_count();

        //The poses of the sonars, it is only interesting the Yaw
        PlayerPose3d [] posesTemp;

        posesTemp = sonar.getGeom().getPoses();

        poseAngles = new double[numReadings];

        //get angle of poses (Yaw)
        for(int i=0;i<numReadings;i++)
        {
            poseAngles[i]=posesTemp[i].getPyaw();

            //print geometry of the sonar sensors
            System.out.println("x: "+Double.toString(posesTemp[i].getPx())+" y: "+Double.toString(posesTemp[i].getPy())+" yaw: "+Double.toString(Math.toDegrees(posesTemp[i].getPyaw())));
        }

        //create data length
        data = new Vector2d[numReadings];

        //TODO: POR REVISAR QUE ES ESTO?????
        this.maxRange = 0.6f;
        this.minRange = 0.2f;
        
    }

    /**
     * Output of this Perception Schema (PS).
     * @return Array of Vector2d, one for each reading.
     */
    public Vector2d[] Value()
    {

        //TODO: Esto deberia corregirse, no deberia ser bloqueante
        //TODO: Sin verificar isDataReady, se tendria lectura del anterior valor.
        // Sin embargo, la primera lectura si es necesario esperar hasta DataReady.
        //Hay que ver como colocar los comportamientos a modo de Hilos.
        //while(!sonar.isDataReady()  && !pos.isDataReady());

        //get data from sonar sensors
        float [] dataTemp = sonar.getData().getRanges();

        //Pose2d locPose = pos.read(Sensor.READ_MODE_NO_WAIT);

        PlayerPose locPose = pos.getData().getPos();

        //as the IRs we have cannot sense distance ranges, set vector mag. as MIN_RANGE.
        for(int i=0;i<numReadings;i++){
           
            data[i] = new Vector2d();

            //The pos of the obstacle is with respect to origin (0,0) define theta with respect to global axes
            data[i].setAngle((float)poseAngles[i]+ (float)locPose.getPa());

            //if there is an obstacle below the MIN_RANGE, signal it.
            if(dataTemp[i]<=minRange) {

                //add robot radius to distance
                data[i].setMag(minRange + Robot.radius);

                //Translate vector from robot coordinates to global coordinates
                data[i] = data[i].add(Position2DHelper.getPosition(pos));
            }
            //Aqui irian las estrategias para que el Sonar no se comporte de manera
            //discreta
            else if(irMode)//if(dataTemp[i] < maxRange)
                data[i].setMag(0);
            else
            {
                if(dataTemp[i]<=maxRange)
                {
                    data[i].setMag(dataTemp[i]+Robot.radius);
                    data[i] = data[i].add(Position2DHelper.getPosition(pos));
                }
                else
                    data[i].setMag(0);
            }

        }
        
        return data;
    }
}
